#include "facegeoestimation.h"
#include "lsqr/LsqrTypes.h"
#include "lsqr/Lsqr.h"
#include "lsqr/SparseMatrix.h"
#include <QMap>
#include <fstream>


FaceGeoEstimation::FaceGeoEstimation()
{
/*    pixelMatrix = NULL;
    lightMatrix = NULL;
    shapeMatrix = NULL;
    singularMatrix = NULL;
    logTextStream = NULL;
*/
    pixelNum = 0;
    imageNum = 0;
    singularNum = 0;
    imageWidth = 0;
    imageHeight = 0;
    iterNum = 0;
}

FaceGeoEstimation::~FaceGeoEstimation()
{
   this->Clear();
}

void FaceGeoEstimation::Clear()
{
    pixelNum = 0;

    pixelPosList.clear();

/*    if(pPixelMatrix)
    {
        delete pPixelMatrix;
        pPixelMatrix = NULL;
    }
    if(lightMatrix)
    {
        delete lightMatrix;
        lightMatrix = NULL;
    }
    if(shapeMatrix)
    {
        delete shapeMatrix;
        shapeMatrix = NULL;
    }
    if(singularMatrix)
    {
        delete singularMatrix;
        singularMatrix = NULL;
    }
*/
}

void FaceGeoEstimation::ReadTemplate(QString fileName)
{

    templateImage.ReadFromFile(fileName);
    imageWidth = templateImage.GetWidth();
    imageHeight = templateImage.GetHeight();
    for(short i=0; i<imageWidth; i++)
    {
        for(short j=0; j<imageHeight; j++)
        {
            QVector3D color = templateImage.GetPixelValue(i,j);
            if((color.x()+color.y()+color.z())>2.5f)
            {
                QVector2D uv(i,j);
                pixelPosList.append(uv);
            }
        }
    }
    pixelNum = pixelPosList.size();

    //initialize matrix
    depthMatrix = cv::Mat::zeros(pixelNum, 3, CV_32F);
    ambiguityMatrix = cv::Mat::zeros(4, 4, CV_32F);
    gbrMatrix = cv::Mat::zeros(3,1,CV_32F);

}

void FaceGeoEstimation::ConstructPixelMatrix(QString imageFilePath, QVector<QString> imageFileNameList, QVector<short> imageIndexList)
{
    if(!pixelPosList.size())
        return;
    imageNum = imageFileNameList.size();
    if(iterNum==0)
        pixelMatrix = cv::Mat::zeros(imageNum, pixelNum, CV_32F);
    for(int i=0; i<imageNum; i++)
    {
        PFMImage tmpImg;
        QString tmpImgName = imageFileNameList[i];

        short dotIndex = tmpImgName.lastIndexOf(".");
        tmpImgName.remove(dotIndex, tmpImgName.size() - dotIndex);
        tmpImgName = imageFilePath + tmpImgName + ".pfm";


        tmpImg.ReadFromFile(tmpImgName);
        for(int j=0; j<pixelNum; j++)
        {
            QVector3D color = tmpImg.GetPixelValue(pixelPosList[j].x(), pixelPosList[j].y());
            float greyValue = 0.299*color.x() + 0.587*color.y() + 0.114*color.z();
            pixelMatrix.at<float>(i,j) = greyValue;
        }
    }
//    this->OutputMatrixToLog(pixelMatrix, "sf");
}

void FaceGeoEstimation::InitialLightShapeEstimation(short singularNumber)
{
    if(!pixelNum || !imageNum)
        return;

//    this->OutputMatrixToLog(pixelMatrix, "sb");

    singularNum = singularNumber;
    cv::Mat u, d, vt;
    cv::SVD::compute(pixelMatrix, d, u, vt);

    if(logTextStream)
    {
        float sigSum = 0.0f;
        for(int k=0; k<10; k++)
        {
            sigSum += d.at<float>(k,0);
            *logTextStream<<d.at<float>(k,0)<<",";
        }
        *logTextStream<<"\n";
         *logTextStream<<sigSum<<"\n";
    }

//    this->OutputMatrixToLog(*pPixelMatrix, "M");
//    this->OutputMatrixToLog(u, "u");
//    this->OutputMatrixToLog(d,"d");
//    this->OutputMatrixToLog(vt,"vt");

    if(iterNum==0)
    {
        lightMatrix = cv::Mat::zeros(imageNum, singularNum, CV_32F);
        shapeMatrix = cv::Mat::zeros(singularNum, pixelNum, CV_32F);
        singularMatrix = cv::Mat::zeros(singularNum, singularNum, CV_32F);
    }


    for(short i=0; i<singularNum; i++)
    {
        u.col(i).copyTo(lightMatrix.col(i));
        vt.row(i).copyTo(shapeMatrix.row(i));
        singularMatrix.at<float>(i,i) = sqrt(d.at<float>(i,0));
    }
 //   this->OutputMatrixToLog((lightMatrix), "u41");
 //   this->OutputMatrixToLog((shapeMatrix), "v41");
 //   this->OutputMatrixToLog((singularMatrix), "s41");
    lightMatrix = lightMatrix * singularMatrix;
    shapeMatrix = singularMatrix * shapeMatrix;

}

void FaceGeoEstimation::OutputReconstructFaceSVD(QString imageFilePath, QVector<QString> imageFileNameList)
{
    if(!pixelNum)
        return;

    cv::Mat pixelMatrixRecons(imageNum, pixelNum, CV_32F);
    pixelMatrixRecons = lightMatrix * shapeMatrix;

    for(int i=0; i<imageNum; i++)
    {

        PFMImage tmpImg(imageWidth, imageHeight, 3, 0);
        for(int j=0; j<pixelNum; j++)
        {
            QVector2D uv = pixelPosList[j];
            float color = pixelMatrixRecons.at<float>(i,j);
            tmpImg.SetPixelValue(uv.x(), uv.y(),QVector3D(color, color, color));
        }

        QString tmpImgName = imageFileNameList[i];
        short dotIndex = tmpImgName.lastIndexOf(".");
        tmpImgName.remove(dotIndex, tmpImgName.size() - dotIndex);
        tmpImgName = imageFilePath + tmpImgName + "svd.pfm";
        tmpImg.WriteToFile(tmpImgName);
    }

}

QVector<short> FaceGeoEstimation::SelectImagesForPixel(float threshold, int pixelIndex)
{


    QVector<short> imageList;

    if(!pixelNum && !imageNum)
        return imageList;

    QVector<float> distanceList;
    distanceList.resize(imageNum);
    cv::Mat tmpPixel(imageNum, 1, CV_32F);
    tmpPixel = lightMatrix*shapeMatrix.col(pixelIndex);
    float sum = 0.0f; //for normalize
    for(short i=0; i<imageNum; i++)
    {
        float p1 = pixelMatrix.at<float>(i,pixelIndex);
        float p2 = tmpPixel.at<float>(i,0);
        distanceList[i] = abs(p1-p2);
        sum += (p1-p2)*(p1-p2);
    }
    sum = sqrt(sum);
    for(short i=0; i<imageNum; i++)
    {
        float distance = distanceList[i]/sum;
        if(distance < threshold)
        {
            imageList.append(i);
        }
    }

    return imageList;
}

void FaceGeoEstimation::FaceNormalEstimation(float threshold)
{
    if(!pixelNum && !imageNum)
        return;

    cv::Mat gMat = cv::Mat::zeros(4,4,CV_32F);
    gMat.at<float>(0,0) = -1.0f;
    gMat.at<float>(1,1) = 1.0f;
    gMat.at<float>(2,2) = 1.0f;
    gMat.at<float>(3,3) = 1.0f;

    cv::Mat sMat(4, 1, CV_32F);
    QVector<short> imageList;
    for(int i=0; i<pixelNum; i++)
    {
        imageList.clear();
        imageList = this->SelectImagesForPixel(threshold, i);
        short subImgNum = imageList.size();
        cv::Mat lMat(subImgNum, 4, CV_32F);
        cv::Mat mMat(subImgNum, 1, CV_32F);
        for(short j=0; j<subImgNum; j++)
        {
            float imageIndex = imageList[j];
            lMat.at<float>(j,0) = lightMatrix.at<float>(imageIndex,0);
            lMat.at<float>(j,1) = lightMatrix.at<float>(imageIndex,1);
            lMat.at<float>(j,2) = lightMatrix.at<float>(imageIndex,2);
            lMat.at<float>(j,3) = lightMatrix.at<float>(imageIndex,3);

            mMat.at<float>(j,0) = pixelMatrix.at<float>(imageIndex, i);
        }

        sMat = (lMat.t()*lMat+gMat).inv()*lMat.t()*mMat;

//        this->OutputMatrixToLog(lMat, "l");
//        this->OutputMatrixToLog(gMat,"g");
//        this->OutputMatrixToLog(mMat, "m");
//        this->OutputMatrixToLog(sMat, "s");
        sMat.col(0).copyTo(shapeMatrix.col(i));
    }
//    this->OutputMatrixToLog(shapeMatrix,"sm");

//    this->OutputNormalMap(shapeMatrix, "normalrecon.pfm");
}

void FaceGeoEstimation::SetLogTextStream(QTextStream *p)
{
    logTextStream = p;
}

void FaceGeoEstimation::OutputMatrixToLog(cv::Mat cvMat, QString matName)
{
    if(logTextStream)
    {
        *logTextStream<<matName<<"=[\n";
        short rows = cvMat.rows;
        short cols = cvMat.cols;
        for(short i=0; i<rows; i++)
        {
            for(short j=0; j<cols; j++)
            {
                *logTextStream<<cvMat.at<float>(i,j)<<"\t";
            }
            *logTextStream<<"\n";
        }
        *logTextStream<<"];\n";
        logTextStream->flush();
    }
}



void FaceGeoEstimation::ReComputeLightMatrix()
{
    //测试，此项功能已经集成进函数ComputeAmbiguityMatrix()
    //得到shape matrix后，再重新计算light matrix 通过min||M-LS||^2，也即是min||S^TL^T-M^T||^2
    cv::Mat u, d, vt;
    cv::SVD::compute(shapeMatrix.t(), d, u, vt);

    int dRows = d.rows;
    cv::Mat dMat = cv::Mat::zeros(dRows, 4, CV_32F);
    for(int i=0; i<dRows; i++)
    {
        float tmp = d.at<float>(i,0);
        if(tmp > 0)
        {
            dMat.at<float>(i,i) = 1.0f/tmp;
        }
        else
        {
            dMat.at<float>(i,i) = 0.0f;
        }
    }


     //得到shape matrix后，再重新计算light matrix 通过min||M-LS||^2，也即是min||S^TL^T-M^T||^2
    cv::Mat b = cv::Mat::zeros(pixelNum, 1, CV_32F);
    cv::Mat l(4,1,CV_32F);
    for(int i=0; i<imageNum; i++)
    {
        pixelMatrix.row(i).copyTo(b.col(0));
        l = vt.t() * dMat * u.t() * b;
        l.col(0).copyTo(lightMatrix.row(i));
    }

}

void FaceGeoEstimation::SetShapeMatrixTemplate(cv::Mat shapeMatTemplate)
{
    shapeMatTemplate.copyTo(shapeMatrixTemplate);

}

void FaceGeoEstimation::SetPixelMatrix(cv::Mat pm)
{
    imageNum = pm.rows;
    pixelNum = pm.cols;
    if(iterNum==0)
        pixelMatrix = cv::Mat::zeros(imageNum, pixelNum, CV_32F);
    pm.copyTo(pixelMatrix);
//    this->OutputMatrixToLog(pixelMatrix, "ss");
}

void FaceGeoEstimation::ComputeAmbiguityMatrix()
{
    //利用模板face model计算ambiguity matrix A

    cv::Mat u, d, vt;
    cv::SVD::compute(shapeMatrix.t(), d, u, vt);

//    this->OutputMatrixToLog(shapeMatrix.t(), "s");
//    this->OutputMatrixToLog(u,"u");
//    this->OutputMatrixToLog(d,"d");
//    this->OutputMatrixToLog(vt, "vt");


    int dRows = d.rows;
    cv::Mat dMat = cv::Mat::zeros(dRows, 4, CV_32F);
    for(int i=0; i<dRows; i++)
    {
        float tmp = d.at<float>(i,0);
        if(tmp > 0)
        {
            dMat.at<float>(i,i) = 1.0f/tmp;
        }
        else
        {
            dMat.at<float>(i,i) = 0.0f;
        }
    }

    cv::Mat shapeMatrixInv = vt.t() * dMat * u.t();

//    this->OutputMatrixToLog(vt.t(), "v");
//    this->OutputMatrixToLog(dMat,"dm");


    //得到shape matrix后，再重新计算light matrix 通过min||M-LS||^2，也即是min||S^TL^T-M^T||^2
    lightMatrix = shapeMatrixInv * pixelMatrix.t();
    lightMatrix = lightMatrix.t();

    //通过min||S_t-A*S||^2，也即是min||S^T*A^T - S_t^T||^2 计算ambiguity matix
    ambiguityMatrix = shapeMatrixInv * shapeMatrixTemplate.t();


//    this->OutputMatrixToLog(shapeMatrixTemplate.t(), "st");
//    this->OutputMatrixToLog(ambiguityMatrix, "a");

//    cv::Mat am1;
//    cv::solve(shapeMatrix.t(), shapeMatrixTemplate.t(), am1, cv::DECOMP_NORMAL);
//    this->OutputMatrixToLog(am1,"a1");

    ambiguityMatrix = ambiguityMatrix.t();
    shapeMatrix = ambiguityMatrix * shapeMatrix;
//    this->OutputMatrixToLog(shapeMatrix, "sm");
    cv::Mat ainv = ambiguityMatrix.inv();
//    this->OutputMatrixToLog(ainv, "ainv");
//    this->OutputMatrixToLog(lightMatrix, "l");
    lightMatrix = lightMatrix * ainv;


}

cv::Mat FaceGeoEstimation::DepthFromNormalByIntegration(cv::Mat normalMat)
{
    //size of normalMat 4 X pixelNum


    short w = templateImage.GetWidth();
    short h = templateImage.GetHeight();
    QMap<int, int> indexMap;
    QVector<QVector2D> indexVector;
    int indexInNormalMat = 0;
    int indexInImg = 0;
    for(short u=0; u<w; u++)
    {
        for(short v=0; v<h; v++)
        {
            QVector3D pixel = templateImage.GetPixelValue(u,v);
            if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
            {
                indexInImg = u*h + v;
                indexMap.insert(indexInImg, indexInNormalMat);

                QVector2D uv;
                uv.setX(u);
                uv.setY(v);
                indexVector.append(uv);

                indexInNormalMat++;
            }
        }
    }

    int pixelNum = indexInNormalMat;
    int rows = 2*pixelNum;
    int cols = pixelNum;    
    SparseMatrix* pSparseMat = new SparseMatrix(rows, cols);
    pSparseMat->SetLogTextStream(logTextStream);
    QVector<int> isolatePX;
    QVector<int> isolatePY;
    for(int i=0; i<pixelNum; i++)
    {

        float nz = normalMat.at<float>(3,i);

        QVector2D uv = indexVector[i];
        QVector3D pixel;
        int tmpIndex = 0;
        bool isolateFlag = false;
        if(uv.x() == (w-1))
        {
           pixel = templateImage.GetPixelValue(uv.x()-1,uv.y());
           if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
           {
               tmpIndex = (uv.x()-1) * h + uv.y();
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2, indexInNormalMat, -nz);
               tmpIndex = uv.x() * h + uv.y();
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2, indexInNormalMat, nz);
           }
           else
           {
               isolatePX.append(i);
               isolateFlag = true;
           }
        }
        else if(uv.x() == 0)
        {
           pixel = templateImage.GetPixelValue(uv.x()+1,uv.y());
           if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
           {
               tmpIndex = (uv.x()+1) * h + uv.y();
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2, indexInNormalMat, nz);
               tmpIndex = uv.x() * h + uv.y();
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2, indexInNormalMat, -nz);
           }
           else
           {
               isolatePX.append(i);
               isolateFlag = true;
           }
        }
        else
        {
            pixel = templateImage.GetPixelValue(uv.x()+1,uv.y());
            if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
            {
               tmpIndex = (uv.x()+1) * h + uv.y();
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2, indexInNormalMat, nz);
               tmpIndex = uv.x() * h + uv.y();
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2, indexInNormalMat, -nz);
            }
            else
            {
                pixel = templateImage.GetPixelValue(uv.x()-1,uv.y());
                if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
                {
                   tmpIndex = (uv.x()-1) * h + uv.y();
                   indexInNormalMat = indexMap[tmpIndex];
                   pSparseMat->SetElement(i*2, indexInNormalMat, -nz);
                   tmpIndex = uv.x() * h + uv.y();
                   indexInNormalMat = indexMap[tmpIndex];
                   pSparseMat->SetElement(i*2, indexInNormalMat, nz);
                }
                else
                {
                    isolatePX.append(i);
                    isolateFlag = true;
                }

            }
        }

        tmpIndex = 0;
        isolateFlag = false;
        if(uv.y() == (h-1))
        {
           pixel = templateImage.GetPixelValue(uv.x(),uv.y()-1);
           if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
           {
               tmpIndex = uv.x() * h + uv.y() - 1;
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2+1, indexInNormalMat, -nz);
               tmpIndex = uv.x() * h + uv.y();
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2+1, indexInNormalMat, nz);
           }
           else
           {
               isolatePY.append(i);
               isolateFlag = true;
           }
        }
        else if(uv.y() == 0)
        {
           pixel = templateImage.GetPixelValue(uv.x(),uv.y()+1);
           if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
           {
               tmpIndex = uv.x() * h + uv.y() + 1;
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2+1, indexInNormalMat, nz);
               tmpIndex = uv.x() * h + uv.y();
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2+1, indexInNormalMat, -nz);
           }
           else
           {
               isolatePY.append(i);
               isolateFlag = true;
           }
        }
        else
        {
            pixel = templateImage.GetPixelValue(uv.x(),uv.y()+1);
            if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
            {
               tmpIndex = uv.x() * h + uv.y() + 1;
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2+1, indexInNormalMat, nz);
               tmpIndex = uv.x() * h + uv.y();
               indexInNormalMat = indexMap[tmpIndex];
               pSparseMat->SetElement(i*2+1, indexInNormalMat, -nz);
            }
            else
            {
                pixel = templateImage.GetPixelValue(uv.x(),uv.y()-1);
                if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
                {
                   tmpIndex = uv.x() * h + uv.y() - 1;
                   indexInNormalMat = indexMap[tmpIndex];
                   pSparseMat->SetElement(i*2+1, indexInNormalMat, -nz);
                   tmpIndex = uv.x() * h + uv.y();
                   indexInNormalMat = indexMap[tmpIndex];
                   pSparseMat->SetElement(i*2+1, indexInNormalMat, nz);
                }
                else
                {
                    isolatePY.append(i);
                    isolateFlag = true;
                }

            }
        }

    }

    //处理左右或上下无相邻点的孤立点，找到上下或左右相邻点，把相邻点的方程拷贝到孤立点所在位置
    for(int i=0; i<isolatePX.size(); i++)
    {
        int index = isolatePX[i];
        int indexTmp = 0;
        QVector2D uv = indexVector[index];
        if(uv.y() == 0)
        {
            indexTmp = uv.x()*h + uv.y() + 1;
        }
        else if(uv.y() == h-1)
        {
            indexTmp = uv.x()*h + uv.y() - 1;
        }
        else
        {
            QVector3D pixel = templateImage.GetPixelValue(uv.x(), uv.y()-1);
            if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
            {
                indexTmp = uv.x()*h + uv.y() - 1;
            }
            else
            {
                indexTmp = uv.x()*h + uv.y() + 1;

            }
        }
        indexInNormalMat = indexMap[indexTmp];
        pSparseMat->RowCopy(indexInNormalMat*2, index*2);
    }

    for(int i=0; i<isolatePY.size(); i++)
    {
        int index = isolatePY[i];
        int indexTmp = 0;
        QVector2D uv = indexVector[index];
        if(uv.x() == 0)
        {
            indexTmp = (uv.x()+1)*h + uv.y();

        }
        else if(uv.x() == w-1)
        {
            indexTmp = (uv.x()-1)*h + uv.y();
        }
        else
        {
            QVector3D pixel = templateImage.GetPixelValue(uv.x()-1, uv.y());
            if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
            {
                indexTmp = (uv.x()-1)*h + uv.y();
            }
            else
            {
                indexTmp = (uv.x()+1)*h + uv.y();
            }
        }
        indexInNormalMat = indexMap[indexTmp];
        pSparseMat->RowCopy(indexInNormalMat*2+1, index*2+1);
    }






    /*
    SparseMatrix* pSparseMat = new SparseMatrix(rows, cols);
    QVector<int> isolatePX;
    QVector<int> isolatePY;
    for(int i=0; i<pixelNum; i++)
    {
        float nx = normalMat.at<float>(i,0);
        float ny = normalMat.at<float>(i,1);

        float cofficient1 = ny - nx;
        pSparseMat->SetElement(i,i,cofficient1);


        QVector2D uv = indexVector[i];
        QVector3D pixel;
        int tmpIndex = 0;
        bool isolateFlag = false;
        if(uv.x() == (w-1))
        {
           pixel = templateImg.GetPixelValue(uv.x()-1,uv.y());
           if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
           {
               tmpIndex = (uv.x()-1) * h + uv.y();
           }
           else
           {
               isolatePX.append(i);
               isolateFlag = true;
           }
        }
        else if(uv.x() == 0)
        {
           pixel = templateImg.GetPixelValue(uv.x()+1,uv.y());
           if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
           {
               tmpIndex = (uv.x()+1) * h + uv.y();
           }
           else
           {
               isolatePX.append(i);
               isolateFlag = true;
           }
        }
        else
        {
            pixel = templateImg.GetPixelValue(uv.x()+1,uv.y());
            if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
            {
               tmpIndex = (uv.x()+1) * h + uv.y();
            }
            else
            {
                pixel = templateImg.GetPixelValue(uv.x()-1,uv.y());
                if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
                {
                   tmpIndex = (uv.x()-1) * h + uv.y();
                }
                else
                {
                    isolatePX.append(i);
                    isolateFlag = true;
                }

            }
        }
        float cofficient2 = -ny;
        if(!isolateFlag)
        {
            indexInNormalMat = indexMap[tmpIndex];
            pSparseMat->SetElement(i, indexInNormalMat, cofficient2);
        }

        tmpIndex = 0;
        isolateFlag = false;
        if(uv.y() == (h-1))
        {
           pixel = templateImg.GetPixelValue(uv.x(),uv.y()-1);
           if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
           {
               tmpIndex = uv.x() * h + uv.y() - 1;
           }
           else
           {
               isolatePY.append(i);
               isolateFlag = true;
           }
        }
        else if(uv.y() == 0)
        {
           pixel = templateImg.GetPixelValue(uv.x(),uv.y()+1);
           if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
           {
               tmpIndex = uv.x() * h + uv.y() + 1;
           }
           else
           {
               isolatePY.append(i);
               isolateFlag = true;
           }
        }
        else
        {
            pixel = templateImg.GetPixelValue(uv.x(),uv.y()+1);
            if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
            {
               tmpIndex = uv.x() * h + uv.y() + 1;
            }
            else
            {
                pixel = templateImg.GetPixelValue(uv.x(),uv.y()-1);
                if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
                {
                   tmpIndex = uv.x() * h + uv.y() - 1;
                }
                else
                {
                    isolatePY.append(i);
                    isolateFlag = true;
                }

            }
        }
        float cofficient3 = nx;
        if(!isolateFlag)
        {
            indexInNormalMat = indexMap[tmpIndex];
            pSparseMat->SetElement(i, indexInNormalMat, cofficient3);
        }

    }

    //处理左右或上下无相邻点的孤立点，找到上下或左右相邻点，把相邻点的方程拷贝到孤立点所在位置
    for(int i=0; i<isolatePX.size(); i++)
    {
        int index = isolatePX[i];
        int indexTmp = 0;
        QVector2D uv = indexVector[index];
        if(uv.y() == 0)
        {
            indexTmp = uv.x()*h + uv.y() + 1;
        }
        else if(uv.y() == h-1)
        {
            indexTmp = uv.x()*h + uv.y() - 1;
        }
        else
        {
            QVector3D pixel = templateImg.GetPixelValue(uv.x(), uv.y()-1);
            if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
            {
                indexTmp = uv.x()*h + uv.y() - 1;
            }
            else
            {
                indexTmp = uv.x()*h + uv.y() + 1;

            }
        }
        indexInNormalMat = indexMap[indexTmp];
        pSparseMat->RowCopy(indexInNormalMat, index);
    }

    for(int i=0; i<isolatePY.size(); i++)
    {
        int index = isolatePY[i];
        int indexTmp = 0;
        QVector2D uv = indexVector[index];
        if(uv.x() == 0)
        {
            indexTmp = (uv.x()+1)*h + uv.y();

        }
        else if(uv.x() == w-1)
        {
            indexTmp = (uv.x()-1)*h + uv.y();
        }
        else
        {
            QVector3D pixel = templateImg.GetPixelValue(uv.x()-1, uv.y());
            if((pixel.x() + pixel.y() + pixel.z()) >2.5f)
            {
                indexTmp = (uv.x()-1)*h + uv.y();
            }
            else
            {
                indexTmp = (uv.x()+1)*h + uv.y();
            }
        }
        indexInNormalMat = indexMap[indexTmp];
        pSparseMat->RowCopy(indexInNormalMat, index);
    }
    */




    //求解min||AX-b||

    FILE* pfile = fopen("lsqr.txt", "w");  
    pSparseMat->TransferToCRS_CCSStroage();
    Lsqr* pLsqr = new Lsqr(pSparseMat);
    pLsqr->allocLsqrMem();
    pLsqr->GetInputPara()->lsqr_fp_out = pfile;

    for(int i=0; i<pixelNum; i++)
    {
        pLsqr->GetInputPara()->rhs_vec->elements[i*2] = -normalMat.at<float>(1,i);
        pLsqr->GetInputPara()->rhs_vec->elements[i*2+1] = -normalMat.at<float>(2,i);
    }

    //debug begin
/*    pSparseMat->OutputMatrix("a.m");
    std::ofstream ofile1("rhs.m");
    ofile1<<"b=[\n";
    for(int i=0; i<pixelNum; i++)
    {
        ofile1<<pLsqr->GetInputPara()->rhs_vec->elements[i*2]<<std::endl;
        ofile1<<pLsqr->GetInputPara()->rhs_vec->elements[i*2+1]<<std::endl;
    }
    ofile1<<"];";
    ofile1.close();

    std::ifstream ifile("rmatlab.txt");
    for(int i=0; i<indexVector.size(); i++)
    {
        QVector3D pixel;
        float value = 0.0;
        ifile>>value;
        pixel.setX(value);
        pixel.setY(value);
        pixel.setZ(value);
        templateImg.SetPixelValue(indexVector[i].x(),indexVector[i].y(), pixel);

    }
    ifile.close();
    templateImg.WriteToFile("depthMapMatlab.pfm");


    //debug end
*/

    for(int i=0; i<cols; i++)
    {
        pLsqr->GetInputPara()->sol_vec->elements[i] = 0.0;
    }

    pLsqr->GetInputPara()->num_rows = rows;
    pLsqr->GetInputPara()->num_cols = cols;
    pLsqr->GetInputPara()->damp_val = 0.0;
    pLsqr->GetInputPara()->rel_mat_err = 1.0e-6;
    pLsqr->GetInputPara()->rel_rhs_err = 1.0e-6;
    pLsqr->GetInputPara()->cond_lim = 0;
    pLsqr->GetInputPara()->max_iter = 500;




 //   t.restart();

    pLsqr->do_lsqr(pSparseMat);

    //debug begin
/*  QString str = QString("lsqr %1\n").arg(t.elapsed());
  if(logTextStream)
  {
       *logTextStream<<str;
      logTextStream->flush();
  }*/
  //debug end


    fprintf( pLsqr->GetInputPara()->lsqr_fp_out, "\t\t\tResidual Norm       Residual Norm       Solution Norm\n" );
    fprintf( pLsqr->GetInputPara()->lsqr_fp_out, "\t\t\t||A x - b||_2   ||A^T A x - A^T b||_2   ||x - x0||_2\n\n" );
    fprintf( pLsqr->GetInputPara()->lsqr_fp_out, "Estimated by LSQR  %17.5e   %17.5e   %17.5e\n",
         pLsqr->GetOutputPara()->resid_norm, pLsqr->GetOutputPara()->mat_resid_norm, pLsqr->GetOutputPara()->sol_norm );


    //subtract the maximum depth value
    float minValue = 100000.0f;
    float maxValue = -100000.0f;
    for(int i=0; i<pLsqr->GetOutputPara()->sol_vec->length; i++)
    {
        float value = pLsqr->GetOutputPara()->sol_vec->elements[i];
        if(value < minValue)
            minValue = value;
        if(value > maxValue)
            maxValue = value;
    }

    for(int i=0; i<pLsqr->GetOutputPara()->sol_vec->length; i++)
    {
        pLsqr->GetOutputPara()->sol_vec->elements[i] -= minValue;
    }


    //debug begin
/*    std::ofstream oFile("r.txt");
    oFile<<"r=[\n";
    for(int i=0; i<pLsqr->GetOutputPara()->sol_vec->length; i++)
    {
        oFile<<pLsqr->GetOutputPara()->sol_vec->elements[i]<<std::endl;
    }
    oFile<<"];\n";
    oFile.close();


    for(int i=0; i<indexVector.size(); i++)
    {
        QVector3D pixel;
        float value = pLsqr->GetOutputPara()->sol_vec->elements[i];
        pixel.setX(value);
        pixel.setY(value);
        pixel.setZ(value);
        templateImg.SetPixelValue(indexVector[i].x(),indexVector[i].y(), pixel);

    }
    templateImg.WriteToFile("depthMap.pfm");*/
    //debug end

    cv::Mat x = cv::Mat::zeros(pixelNum, 1, CV_32F);
    for(int i=0; i<pixelNum; i++)
    {
        x.at<float>(i,0) = pLsqr->GetOutputPara()->sol_vec->elements[i];
        depthMatrix.at<float>(i,2) = pLsqr->GetOutputPara()->sol_vec->elements[i];
    }

    fclose(pfile);
    delete pSparseMat;
    pLsqr->freeLsqrMem();
    delete pLsqr;

    return x;

}

void FaceGeoEstimation::ReadReferenceGeoImage(QString fileName)
{
    referenceGeoImage.ReadFromFile(fileName);
    for(int i=0; i<pixelNum; i++)
    {
        QVector2D uv = pixelPosList[i];
        QVector3D pixel = referenceGeoImage.GetPixelValue(uv.x(),uv.y());
        depthMatrix.at<float>(i,0) = pixel.x();
        depthMatrix.at<float>(i,1) = pixel.y();

    }

}

void FaceGeoEstimation::SetGBRCofficient(QVector3D co)
{
    gbrMatrix.at<float>(0,0) = co.x();
    gbrMatrix.at<float>(1,0) = co.y();
    gbrMatrix.at<float>(2,0) = co.z();

}

QVector3D FaceGeoEstimation::ComputeGBRCofficient(cv::Mat depth)
{   

    cv::Mat a = cv::Mat::zeros(pixelNum, 3, CV_32F);
    cv::Mat b = cv::Mat::zeros(pixelNum, 1, CV_32F);

    depth.col(0).copyTo(a.col(2));

    for(int i=0; i<pixelNum; i++)
    {
        QVector2D uv = pixelPosList[i];
        QVector3D pixel = referenceGeoImage.GetPixelValue(uv.x(),uv.y());
        a.at<float>(i,0) = pixel.x();
        a.at<float>(i,1) = pixel.y();
        b.at<float>(i,0) = pixel.z();
    }

    cv::solve(a,b,gbrMatrix,cv::DECOMP_SVD);
    QVector3D cofficients;
    cofficients.setX(gbrMatrix.at<float>(0,0));
    cofficients.setY(gbrMatrix.at<float>(1,0));
    cofficients.setZ(gbrMatrix.at<float>(2,0));


    //debug begin
/*    cv::Mat br;
    br = a*gbrMatrix;
    for(int i=0; i<pixelNum; i++)
    {
        QVector2D uv = pixelPosList[i];
        QVector3D pixel;
        pixel.setX(a.at<float>(i,0));
        pixel.setY(a.at<float>(i,1));
        pixel.setZ(br.at<float>(i,0));
        referenceGeo.SetPixelValue(uv.x(),uv.y(),pixel);

    }
    referenceGeo.WriteToFile("reconGeo.pfm");*/
 //   this->OutputMatrixToLog(a,"a");
 //   this->OutputMatrixToLog(b,"b");
 //   this->OutputMatrixToLog(br,"br");

    //debug end

    return cofficients;

}

void FaceGeoEstimation::ReconstructDepthMapByIntegration()
{
    cv::Mat scaleDepth = depthMatrix*gbrMatrix;
    scaleDepth.col(0).copyTo(depthMatrix.col(2));

    /*
     * test lsqr algorithm
     *
    int rows = 1000;
    int cols = 1000;
    SparseMatrix* sm = new SparseMatrix(rows, cols);
    FILE* pfile = fopen("t.txt", "w");
    std::ifstream iFile("a.txt");
    for(int i=0; i<rows; i++)
    {
        float value = 0;
        iFile>>value;
        sm->SetElement(i,i,value);
    }
    iFile.close();

    Lsqr* pLsqr = new Lsqr(sm);
    pLsqr->allocLsqrMem();
    pLsqr->GetInputPara()->lsqr_fp_out = pfile;
    dvec* rhsvec = (dvec*) alloc_dvec(cols);
    iFile.open("b.txt");
    for(int i=0; i<cols; i++)
    {
        float value = 0;
        iFile>>value;
        rhsvec->elements[i] = value;
    }
    iFile.close();
    dvec_copy(rhsvec, pLsqr->GetInputPara()->rhs_vec);


    for(int i=0; i<cols; i++)
    {
        pLsqr->GetInputPara()->sol_vec->elements[i] = 0.0;
    }

    pLsqr->GetInputPara()->num_rows = rows;
    pLsqr->GetInputPara()->num_cols = cols;
    pLsqr->GetInputPara()->damp_val = 0.0;
    pLsqr->GetInputPara()->rel_mat_err = 1.0e-6;
    pLsqr->GetInputPara()->rel_rhs_err = 1.0e-6;
    pLsqr->GetInputPara()->cond_lim = 0;
    pLsqr->GetInputPara()->max_iter = rows+cols;

    pLsqr->do_lsqr(sm);

    fprintf( pLsqr->GetInputPara()->lsqr_fp_out, "\t\t\tResidual Norm       Residual Norm       Solution Norm\n" );
    fprintf( pLsqr->GetInputPara()->lsqr_fp_out, "\t\t\t||A x - b||_2   ||A^T A x - A^T b||_2   ||x - x0||_2\n\n" );
    fprintf( pLsqr->GetInputPara()->lsqr_fp_out, "Estimated by LSQR  %17.5e   %17.5e   %17.5e\n",
         pLsqr->GetOutputPara()->resid_norm, pLsqr->GetOutputPara()->mat_resid_norm, pLsqr->GetOutputPara()->sol_norm );

    std::ofstream oFile("r.txt");
    oFile<<"r=[\n";
    for(int i=0; i<cols; i++)
    {
        oFile<<pLsqr->GetOutputPara()->sol_vec->elements[i]<<std::endl;
    }
    oFile<<"];\n";
    oFile.close();

    free_dvec(rhsvec);
    fclose(pfile);
    delete sm;
    pLsqr->freeLsqrMem();
    delete pLsqr;
    */

}

void FaceGeoEstimation::OutputReconGeo(QString fileName)
{
    PFMImage geoMap(256, 256, 3, 0);
    QVector3D geo;
    float tmpValue;
    for(int i=0; i<pixelNum; i++)
    {
        QVector2D uv = pixelPosList[i];
        tmpValue = depthMatrix.at<float>(i, 0);
        geo.setX(tmpValue);

        tmpValue = depthMatrix.at<float>(i, 1);
        geo.setY(tmpValue);

        tmpValue = depthMatrix.at<float>(i, 2);
        geo.setZ(tmpValue);

        geoMap.SetPixelValue(uv.x(), uv.y(), geo);

    }
    geoMap.WriteToFile(fileName);

}

void FaceGeoEstimation::OutputNormalMap(cv::Mat shapeMatrix4XN, QString fileName)
{
    PFMImage normalMap(256, 256, 3, 0);
    QVector3D normal;
    float tmpValue;
    for(int i=0; i<pixelNum; i++)
    {
        QVector2D uv = pixelPosList[i];
        tmpValue = shapeMatrix4XN.at<float>(1, i);
        normal.setX(tmpValue);

        tmpValue = shapeMatrix4XN.at<float>(2, i);
        normal.setY(tmpValue);

        tmpValue = shapeMatrix4XN.at<float>(3, i);
        normal.setZ(tmpValue);

        normal.normalize();

        normalMap.SetPixelValue(uv.x(), uv.y(), normal);

    }
    normalMap.WriteToFile(fileName);

}



